﻿using CrankShaftAnalysisSystem.Devices;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading;
using System.Threading.Tasks;

namespace CrankShaftAnalysisSystem.Controller
{
    public class PressureController
    {
        public const int Axis = 3; // 3号电机控制压力
        public static int MovingRPM = 800;
        public const double MaxMoveHeight = 100; // 单次移动最多的距离100mm
        public static MeasureSystem.State State { get; private set; } = MeasureSystem.State.IDLE;
        /// <summary>
        /// 估算出来的高度值
        /// </summary>
        public static double Height { get; private set; }
        /// <summary>
        /// 移动到压紧
        /// </summary>
        /// <param name="target_press">压缩率，必须小于1800</param>
        /// <returns></returns>
        public static Task PressPart(double target_press = -1)
        {
            DistanceEastenGauge Easten = DistanceEastenGauge.Instance;
            if (State == MeasureSystem.State.BUSY)
            {
                throw new Exception("Pressure Controller Busy");
            }
            if(target_press<0)
            {
                target_press = Easten.PenTargetCompression * 0.9;
            }
            double max_press = Easten.PenTargetCompression * 0.9;
            if (Math.Abs(target_press) > max_press) target_press = max_press;
            State = MeasureSystem.State.BUSY;
            const int delay_time = 100;

            return Task.Run(() =>
            {
                if(!Easten.PenCompressionReached())
                    PLCMaster.Instance.SetSpeed(Axis, -MovingRPM);
                while (true)
                {
                    var p1 = Easten.GetPenCompression(0);
                    var p2 = Easten.GetPenCompression(1);
                    if(p1>target_press||p2>target_press)
                    {
                        Console.WriteLine($"p1: {p1}, p2: {p2}, Finish.");
                        Height = 0;
                        break;
                    }
                    if (SafeController.ManualStop)
                    {
                        Console.WriteLine("[Safe Controller]: StopAll.");
                        PLCMaster.Instance.SetSpeed(Axis, 0);
                        break;
                    }
                    // 圈/min / 60 = 圈/s, 圈/s * time * 导程（10um） = mm
                    Height = Height - MovingRPM / 79.0/ 60.0 * delay_time / 1000.0 * 10.0;
                    Thread.Sleep(delay_time);
                }
                PLCMaster.Instance.SetSpeed(Axis, 0);
                State = MeasureSystem.State.FINISHED;
            });
        }

        public static Task MoveUp(double target_height = 25)
        {
            DistanceEastenGauge Easten = DistanceEastenGauge.Instance;
            if (State == MeasureSystem.State.BUSY)
            {
                throw new Exception("Pressure Controller Busy");
            }
            target_height = Math.Abs(target_height);
            if (target_height > MaxMoveHeight) target_height = MaxMoveHeight;
            State = MeasureSystem.State.BUSY;
            const int delay_time = 100;

            return Task.Run(() =>
            {
                if (!Easten.PenCompressionReached())
                {
                    Height = 0;
                }
                PLCMaster.Instance.SetSpeed(Axis, MovingRPM);
                while (true)
                {
                    if (Height > target_height)
                    {
                        Console.WriteLine($"Current Height: {Height}, Finish.");
                        break;
                    }
                    if (SafeController.ManualStop)
                    {
                        Console.WriteLine("[Safe Controller]: StopAll.");
                        PLCMaster.Instance.SetSpeed(Axis, 0);
                        break;
                    }
                    // 圈/min / 60 / 减速比（79） = 圈/s, 圈/s * time * 导程（10um） = mm
                    Height = Height + MovingRPM / 79.0 / 60.0 * delay_time / 1000.0 * 10.0;
                    Thread.Sleep(delay_time);
                }
                PLCMaster.Instance.SetSpeed(Axis, 0);
                State = MeasureSystem.State.FINISHED;
            });
        }

    }
}
